/*!
  ******************************************************************************
  * @file           : protocol.cpp
  * @author         :He  Kunling
  * @createDate     : 2021/07/31
  * @lastEditors    :
  * @lastEditTime   : 2021/08/01
  * @brief          : 
  * 
  *     This file provides functions bellow to be called:
  * 
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2020 Zero_One NUAA.
  * All rights reserved.
  *
  ******************************************************************************
  */
#include "main.hpp"
#include <iostream>
/************************************************************/
/***************************************************************/
int32_t init_alt = 0;
uint8_t init_alt_time = 0;

uint8_t waypoints_cmd[4];                            //航点控制指令       指令只存在前两个字节
bool waypoints_cmd_flag = false;

uint8_t waypoints_single[18];                      //单条航点位置信息      指令只存在前16个字节
bool waypoints_single_flag = false;

uint8_t waypoints_tracking[14];                 
bool waypoints_tracking_flag = false;

bool all_waypoint_OK = false;


/********************************************************/

uint8_t waypoints_cmd_feedback[4];                //飞控返回的航点控制指令反馈        只用前两个字节
bool waypoints_cmd_feedback_flag = false;

uint8_t waypoints_feedback[3];                          //飞控反馈的航点接受状态      只用第一个字节
bool waypoints_feedback_flag = false;

uint8_t waypoints_single_feedback[18];         //单条航点的信息反馈       只用前16个字节
bool waypoints_single_feedback_flag = false;
/*****************************************************/


 uint8_t set_head(uint8_t* buffer, uint8_t index, 
                 uint8_t head1, uint8_t head2,uint8_t length, uint8_t category, uint8_t flightID)
 {
    buffer[index++] = head1;
    buffer[index++] = head2;
    buffer[index++] = length;
    buffer[index++] = category;
    buffer[index++] = flightID;

    return index;
 }

 uint8_t set_low_freq_onboard_to_ground_payload(uint8_t* buffer,uint8_t index,uint8_t selfcheck, uint8_t flight_mode, 
                uint8_t flight_status, uint8_t fix_status, uint8_t battery)
 {
     uint16_t temp = 0;

    buffer[index++] = selfcheck;
    buffer[index++] = flight_mode;
    buffer[index++] = flight_status;
    buffer[index++] = fix_status;
     buffer[index++] = battery;

     return index;
 }

 uint8_t set_high_freq_onboard_to_ground_payload(uint8_t* buffer,uint8_t index,uint16_t roll, uint16_t pitch, 
                uint16_t yaw, uint16_t vel_xy, uint16_t vel_z,int32_t lat , int32_t lng, int32_t alt)
 {
     uint16_t temp_16 = 0;
     int32_t temp_32 = 0;

     temp_16 = roll;
     buffer[index++] =  USR_BYTE1(temp_16);
     buffer[index++] =  USR_BYTE0(temp_16);

     temp_16 = pitch;
     buffer[index++] =  USR_BYTE1(temp_16);
     buffer[index++] =  USR_BYTE0(temp_16);

     temp_16 = yaw;
     buffer[index++] =  USR_BYTE1(temp_16);
     buffer[index++] =  USR_BYTE0(temp_16);

     temp_16 = vel_xy;
     buffer[index++] =  USR_BYTE1(temp_16);
     buffer[index++] =  USR_BYTE0(temp_16);

     temp_16 = vel_z;
     buffer[index++] =  USR_BYTE1(temp_16);
     buffer[index++] =  USR_BYTE0(temp_16);

    temp_32 = lat;
     buffer[index++] =  USR_BYTE3(temp_32);
     buffer[index++] =  USR_BYTE2(temp_32);
     buffer[index++] =  USR_BYTE1(temp_32);
     buffer[index++] =  USR_BYTE0(temp_32);

    temp_32 = lng;
     buffer[index++] =  USR_BYTE3(temp_32);
     buffer[index++] =  USR_BYTE2(temp_32);
     buffer[index++] =  USR_BYTE1(temp_32);
     buffer[index++] =  USR_BYTE0(temp_32);

    temp_32 = alt;
     buffer[index++] =  USR_BYTE3(temp_32);
     buffer[index++] =  USR_BYTE2(temp_32);
     buffer[index++] =  USR_BYTE1(temp_32);
     buffer[index++] =  USR_BYTE0(temp_32);
     return index;
 }

uint8_t set_onboard_to_flight_control_payload(uint8_t* buffer, uint8_t index,
                uint8_t cmd, int16_t target_x, int16_t target_y, uint8_t speed)
{
    buffer[index++] =  cmd;

    buffer[index++] =  USR_BYTE1(target_x);
    buffer[index++] =  USR_BYTE0(target_x);
    buffer[index++] =  USR_BYTE1(target_y);
    buffer[index++] =  USR_BYTE0(target_y);

    buffer[index++] =  speed;
     return index;

}

uint8_t set_checksum(uint8_t* buffer, uint8_t index)
{
    int16_t temp_check = 0;

    for(uint8_t i=0; i<index; ++i){
        temp_check += buffer[i];
    }

    buffer[index++] = USR_BYTE1(temp_check);
    buffer[index++] = USR_BYTE0(temp_check);

    return index;
}

bool get_checksum(uint8_t* buffer, uint8_t length)
{
    int16_t  temp_check = 0;

    for(uint8_t i=0; i<(length-2); ++i){
        temp_check += buffer[i];
    }
    uint8_t high = USR_BYTE1(temp_check);
    uint8_t low  = USR_BYTE0(temp_check);

    if(high!=buffer[length-2] || low!=buffer[length-1]){
        for(uint8_t i=0; i<length; ++i){
            buffer[i] = 0;
        }
        return false;
    }
    else return true;

}

uint8_t NX_get_groundbase_cmd(uint8_t* data) 
{
    if(data[3] == 0xA3){
        std::cout << "Receive  ground cmd\n "<< std::endl;
         if(data[4] != usr_flag.flight_ID){
            return 0;
        }
        if(get_checksum(data,data[2]) == false){
            std::cout << "warning:ground cmd receive CHECKSUM false!\n"<< std::endl;
            return 0;
        }
        usr_flag.NX_wait_groundbase_controlcmd_ready_flag = 0;
        groundbase_cmd.control_cmd = data[5];
        printf( "22222222222222222222222groundbase_cmd.control_cmd = %x \n",groundbase_cmd.control_cmd );
        switch(groundbase_cmd.control_cmd)
        {    
                case 0x00: {
                    std::cout << "disarm the drone\n" << std::endl;

                    
                    break;
                }
                case 0x01: {
                    std::cout << "arm the drone\n" << std::endl;

                    break;
                }
                case 0x02: {
                    std::cout << "takeoff!!!\n" << std::endl;

                    break;
                }
                 case 0x03: {
                    std::cout << "land!!!\n" << std::endl;

                    break;
                }
                case 0x04: {
                    std::cout << " RTL!!!\n" << std::endl;

                    break;
                }
                case 0x05: {
                    std::cout << " hovering\n!!!" << std::endl;

                    break;
                }
                case 0x06: {
                    std::cout << "follow!!!\n" << std::endl;

                    break;
                }
                case 0x07: {
                    std::cout << "start auto mission!!!\n" << std::endl;

                    break;
                }
                case 0x09: {
                    std::cout << "start self check\n" << std::endl;
                    break;
                }

                case 0x10: {
                    std::cout << "electromagnet close\n" << std::endl;
                    break;
                }
                case 0x11: {
                    std::cout << "electromagnet open\n" << std::endl;
                    break;
                }
                case 0x12: {
                    std::cout << "son_airplane not take off\n" << std::endl;
                    break;
                }
                case 0x13: {
                    std::cout << "son_airplane take off\n" << std::endl;
                    break;
                }


                case 0x16: {
                    std::cout << "speed forward start !!!\n" << std::endl;   
                    break;
                }
                case 0x17: {
                    std::cout << "speed backward start  !!!\n" << std::endl;
                    break;
                }
                 case 0x18: {
                    std::cout << "speed leftward start  !!!\n" << std::endl;
                    break;
                 }
                case 0x19: {
                    std::cout << "speed rightward start  !!!\n" << std::endl;                                                                            
                    break;
                }      
        }
        ground_cmd_flag = true;
    }
    return data[3];
}


 int receive_all_waypoint_flag = 0;
uint8_t  NX_get_groundbase_waypoints_cmd(uint8_t* data)      //地面站发送的请求下载航点指令
{
    if(data[3] == 0xB1){
         std::cout << "11111111111111111111111111111111111Receive  waypoints cmd data\n ";
         if(data[4] != usr_flag.flight_ID){
             return 0;
         }
         if(get_checksum(data,9) == false){
             std::cout << "warning:ground cmd receive CHECKSUM false!\n"<< std::endl;
            return 0;
        }
         for(uint8_t i = 0;i<4;i++)
         {
                 waypoints_cmd[i] = data[5+i];     //指令只存在前两个字节

         }
        
        NX_get_up_down_waypoint_cnt = 0;
        if(waypoints_cmd[0] == 0x00)
        {

            usr_flag.final_waypoint_flag = false;
            groundbase_waypoint.point_number = waypoints_cmd[1];   //地面站发送的航点总数
            waypoints_cmd_flag = true;
            receive_all_waypoint_flag = 0;
             std::cout << "down1111111!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"<< std::endl;

      
        }else if(waypoints_cmd[0] == 0x01)
        {
            receive_all_waypoint_flag++;
            waypoints_single_flag = false;
            waypoints_cmd_flag = false;
            if(receive_all_waypoint_flag == 1)
            {
               all_waypoint_OK = true;
            }
         

        }
        else if(waypoints_cmd[0] == 0x02)  ///地面站请求上传航点指令
        {
            NX_get_start_up_waypoint_flag = true;  
             NX_get_up_waypoint_single_flag = true;    
             groundbase_cmd.current_up_index = 0;   ///请求第一个航线信息，可以得到航点总数

             std::cout << "up1111111!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"<< std::endl;

       }else if(waypoints_cmd[0] == 0x03)///地面站请求上传单个航点信息指令
        {
             printf("!cccc!groundbase_waypoint.point_number = %d\n", data[6]);
              NX_get_up_waypoint_single_flag = true;
            groundbase_cmd.current_up_index = data[6]-1;
            if(groundbase_cmd.current_up_index == flight_control_waypoint.point_number)  ///请求最后一个航点
            {
                NX_get_up_waypoint_final_flag = true;
            }

        }
        else
        {
               waypoints_cmd_flag = false;
        }
     

    }
    return 0;
}



uint8_t NX_get_groundbase_heart_beat(uint8_t* data)
 {
        if(data[3] == 0xA4){
        std::cout << "Receive  ground heart beat\n "<< std::endl;
         if(data[4] != usr_flag.flight_ID){
              std::cout << "Receive  ground heart beat 8\n "<< std::endl;
            return 0;
        }
        if(get_checksum(data,0x08) == false){
            std::cout << "warning:Receive  ground heNX_get_groundbase_heart_beatrt beat CHECKSUM false!\n"<< std::endl;
            return 0;
        }else{
            
            NX_get_up_down_waypoint_cnt = 0;
            if(data[5] == 0){   //yaoce
               usr_flag.gcs_lose_heart_beat_flag = false;
               if_up_and_down_waypoint_falg = false;

            }else if(data[5] == 1){   ///noyaoce
                usr_flag.gcs_lose_heart_beat_flag = false;
                if_up_and_down_waypoint_falg = true;

            }

        }
        
        
         usr_flag.NX_get_groundbase_heartbeat_cnt = 0;
}

 return  data[3];
}



uint8_t  NX_get_groundbase_single_waypoints(uint8_t* data)     //地面站发送的航点信息
{
    uint8_t i;
    if(data[3] == 0xB2){
        std::cout << "Receive  single waypoints data\n ";
        if(data[4] != usr_flag.flight_ID){
            return 0;
        }
        if(get_checksum(data,0x17) == false){
             std::cout << "warning:ground cmd receive CHECKSUM false!\n"<< std::endl;
            return 0;
        }
        for(i=0;i<18;i++)
        {
            waypoints_single[i] = data[5+i];      //指令只存在前16个字节
            printf("waypoints_single[%d] =  %x \n",i,waypoints_single[i]);
        }
       
        
        groundbase_waypoint.current_index = (waypoints_single[0] - 1);//当前航点序号
        printf("groundbase_waypoint.current_index = %d\n",groundbase_waypoint.current_index);
        groundbase_waypoint.next_index = waypoints_single[0];
        groundbase_waypoint.point_mode = waypoints_single[1];//航点类型
        groundbase_waypoint.point_lat = ((waypoints_single[2]<<24)|(waypoints_single[3]<<16)|(waypoints_single[4]<<8)|(waypoints_single[5]));
        groundbase_waypoint.point_lng = ((waypoints_single[6]<<24)|(waypoints_single[7]<<16)|(waypoints_single[8]<<8)|(waypoints_single[9]));
        groundbase_waypoint.point_alt = ((waypoints_single[10]<<24)|(waypoints_single[11]<<16)|(waypoints_single[12]<<8)|(waypoints_single[13]))*10; //单位：厘米mm
        groundbase_waypoint.point_duration = (waypoints_single[14]<<8)|waypoints_single[15];

        waypoints_single_flag = true;
         NX_get_up_down_waypoint_cnt = 0;

         std::cout << "Receive  single waypoints data completely \n ";
         printf("groundbase_waypoint.current_index = %d\n",groundbase_waypoint.current_index);
           printf("groundbase_waypoint.point_lat = %d\n",groundbase_waypoint.point_lat );
             printf("groundbase_waypoint.point_lng = %d\n",groundbase_waypoint.point_lng);

    }
    return 0;
}

uint8_t NX_send_waypoint_to_flight_control(LinuxSerialDevice* radio)   //向飞控上传航点
{
    static uint16_t crc;
    static uint8_t index = 63;
    static uint8_t send_data[63] = {0};
    static int16_t i_lon = 0;
    static int32_t f_lon = 0;
    static int16_t i_lat = 0;
    static int32_t f_lat = 0;
    static int32_t altitude = 0;
    static int8_t sum_send_time = 0;



    if(waypoints_single_flag == true)
    {

        send_data[0] = 0x43;
        send_data[1] = 0x57;
        send_data[2] = 56;
        send_data[3] = 0x00;
        send_data[4] = 0x08;
        send_data[5] = groundbase_waypoint.point_number&0xFF;
        send_data[6] = (groundbase_waypoint.point_number>>8)&0xFF;
        send_data[7] = groundbase_waypoint.current_index&0xFF;
        send_data[8] = (groundbase_waypoint.current_index>>8)&0xFF;
        if(groundbase_waypoint.point_number == (groundbase_waypoint.current_index + 1))    //final waypoint
        {
            send_data[9] = 0;
            send_data[10] = 0;
        }
        else
        {
            send_data[9] = groundbase_waypoint.next_index&0xFF;
            send_data[10] = (groundbase_waypoint.next_index>>8)&0xFF;
        }
        
        send_data[11] = 0;

        i_lon = (int16_t)(groundbase_waypoint.point_lng/10000000);
        send_data[12] = i_lon&0xFF;
        send_data[13] = (i_lon>>8)&0xFF;
        f_lon = (int32_t)(groundbase_waypoint.point_lng - i_lon*10000000)*100;
        send_data[14] = f_lon&0xFF;
        send_data[15] = (f_lon>>8)&0xFF;
        send_data[16] = (f_lon>>16)&0xFF;
        send_data[17] = (f_lon>>24)&0xFF;

        i_lat = (int16_t)(groundbase_waypoint.point_lat/10000000);
        send_data[18] = i_lat&0xFF;
        send_data[19] = (i_lat>>8)&0xFF;
        f_lat = (int32_t)(groundbase_waypoint.point_lat - i_lat*10000000)*100;
        send_data[20] = f_lat&0xFF;
        send_data[21] = (f_lat>>8)&0xFF;
        send_data[22] = (f_lat>>16)&0xFF;
        send_data[23] = (f_lat>>24)&0xFF;
        send_data[24] = groundbase_waypoint.point_alt&0xFF;
        send_data[25] = (groundbase_waypoint.point_alt>>8)&0xFF;
        send_data[26] = (groundbase_waypoint.point_alt>>16)&0xFF;
        send_data[27] = (groundbase_waypoint.point_alt>>24)&0xFF;

        send_data[28] = 0x32;  //5 m/s
        send_data[29] = 0;

        send_data[30] = 0;
        send_data[31] = 0;

        send_data[32] = 0x32;
        send_data[33] = 0;

        if(groundbase_waypoint.point_mode == 0x02)   //巡航模式
        {
            send_data[34] = 2;//过路模式
        }
        else if(groundbase_waypoint.point_mode == 0x03)   //降落模式
        {
            send_data[34] = 7;  //降落模式
        }
        else if(groundbase_waypoint.point_mode == 0x04)   //悬停模式
        {
            send_data[34] = 4;   //悬停模式
        }
        else if(groundbase_waypoint.point_mode == 0x05)   //返航模式
        {
            send_data[34] = 8;  //返航模式
        }
        else 
        {
             send_data[34] = 4;   //悬停模式
        }

        send_data[35] = groundbase_waypoint.point_duration&0xFF;
        send_data[36] = (groundbase_waypoint.point_duration>>8)&0xFF;

        send_data[37] = 0;
        send_data[38] = 0;

        send_data[39] = 0;
        send_data[40] = 0;
        send_data[41] = 0;
        send_data[42] = 0;
        send_data[43] = 0x64;
        send_data[44] = 0;
        send_data[45] = 0;
        send_data[46] = 0;
        send_data[47] = 0;
        send_data[48] = 0;
        send_data[49] = 0;
        send_data[50] = 0;
        send_data[51] = 0;
        send_data[52] = 0;
        send_data[53] = 0;
        send_data[54] = 0;
        send_data[55] = 0;
        send_data[56] = 0;
        send_data[57] = 0;
        send_data[58] = 0;
        send_data[59] = 0;
        send_data[60] = 0;

        crc = CalculateCrc16(send_data,61);
    
        send_data[61] = (crc >> 8)&0xFF;
        send_data[62] = crc&0xFF;
        index = 63;
        for(sum_send_time = 0;sum_send_time<3;sum_send_time++)
        {
            radio->send(send_data, index);
        }
        
    }
    return index;
}


uint8_t  user_get_flightcontrol_waypoints_cmd_feedback(uint8_t* data)
{
    if(data[3] == 0xB3){
        // std::cout << "Receive  flight contronl single waypoints feedback\n ";
        if(data[4] != usr_flag.flight_ID){
             return 0;
        }
        if(get_checksum(data,0x09) == false){
            return 0;
        }

        waypoints_cmd_feedback[0] = data[5];
        waypoints_cmd_feedback[1] = data[6];        
        waypoints_cmd_feedback[2] = data[7];
        waypoints_cmd_feedback[3] = data[8];    

        printf("waypoints_cmd_feedback[0] = %x \n", waypoints_cmd_feedback[0]);
         printf("waypoints_cmd_feedback[0] = %x \n", waypoints_cmd_feedback[1]);
          printf("waypoints_cmd_feedback[0] = %x \n", waypoints_cmd_feedback[2]);
           printf("waypoints_cmd_feedback[0] = %x \n", waypoints_cmd_feedback[3]);

        waypoints_cmd_feedback_flag  =  true;

        }
        return 0;
}


uint8_t  user_get_flightcontrol_waypoints_feedback(uint8_t* data)
{
    if(data[3] == 0xB4){
        std::cout << "Receive  flight contronl  waypoints feedback\n ";
         if(data[4] != usr_flag.flight_ID){
             return 0;
        }
        if(get_checksum(data,0x08) == false){

            return 0;
        }

        waypoints_feedback[0] = data[5];  
        waypoints_feedback[1] = data[6];  
        waypoints_feedback[2] = data[7];  
        waypoints_feedback_flag = true;
        std::cout << "Receive  flight contronl  waypoints feedback completely\n ";
        }
        return 0;
}


uint8_t  user_get_groundbase_single_waypoints_feedback(uint8_t* data)
{
    if(data[3] == 0xB5){
        if(data[4] != usr_flag.flight_ID){
            return 0;
        }
        
        if(get_checksum(data,23) == false){
            return 0;
            }

    
        std::cout << "Receive  single waypoints feedback\n ";

        for(uint8_t i = 0;i< 18;i++)
        {
            waypoints_single_feedback[i] = data[5+i];
        }
        waypoints_single_feedback_flag = true;
         std::cout << "Receive  waypoints_single_feedback completely \n ";
    }
    return 0;
}



uint8_t user_get_flight_control_data(uint8_t* data)
{
    if(data[3] == 0xD1){
        // std::cout << "Receive  flight control data\n ";
        if(data[4] != usr_flag.flight_ID){
              return 0;
         }
        if(get_checksum(data,0x24) == false){
            // std::cout << "warning flight control data receive CHECKSUM false!\n";
            return 0;
        }

        drone_data_to_groundbase.selfcheck = data[5];
        drone_data_to_groundbase.flight_mode = data[6];
        drone_data_to_groundbase.flight_status = data[7];
        drone_data_to_groundbase.fix_status = data[8];

        drone_data_to_groundbase.roll = data[9]<<8 | data[10];
        drone_data_to_groundbase.pitch = data[11]<<8 | data[12];
        drone_data_to_groundbase.yaw = data[13]<<8 | data[14];

        drone_data_to_groundbase.vel.x = data[15]<<8 | data[16];
        drone_data_to_groundbase.vel.y = data[17] << 8| data[18];
        drone_data_to_groundbase.vel.z = data[19]<< 8 | data[20];

        drone_data_to_groundbase.battery = data[21];

        drone_data_to_groundbase.curr_pos.lat = data[22]<<24 | data[23] <<16 | data[24] << 8 | data[25];
        drone_data_to_groundbase.curr_pos.lng = data[26]<<24 | data[27] <<16 | data[28] << 8 | data[29];
        drone_data_to_groundbase.curr_pos.alt = data[30]<<24 | data[31] <<16 | data[32] << 8 | data[33];


    }
        return data[3];
        
}

uint8_t user_get_flight_control_feedback(uint8_t* data)
{
    if(data[3] == 0xD2){

         if(data[4] != usr_flag.flight_ID){
                // std::cout << "wrong uav id:\n ";
        return 0;
     }
        if(get_checksum(data,0x08) == false){

            return 0;
        }

        drone_data_to_groundbase.cmd_feedback  = data[5];

    }
    return data[3];
}


uint8_t  NX_get_groundbase_tracking_waypoints(uint8_t* data)   //机载计算机获得地面站发出的跟随航点的经纬度
{

    uint8_t i;
    if(data[3] == 0xC0){
        std::cout << "Receive  single waypoints data\n ";
        if(data[4] != usr_flag.flight_ID){
            return 0;
        }
        if(get_checksum(data,19) == false){
            return 0;
        }

        for(i=0;i<14;i++)
        {
            waypoints_tracking[i] = data[5+i];
           printf("waypoints_tracking[%d] =  %x \n",i,waypoints_tracking[i]);
        }
        waypoints_tracking_flag = true;
         std::cout << "Receive  tracking waypoints data completely \n ";   
    }
    return 0;
}



void analyse_groundbase(circleBuf_t* temp, uint8_t* data)    //按协议解析地面站数据
{
    static uint8_t rcvPart = 0;
    static uint8_t index = 0;
    static uint8_t length = 0;

    switch (rcvPart)
    {
        case 0:{
            if(temp->buf[temp->tail] == 0xEB){
                data[index++] = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                rcvPart = 1;
            }
            else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
            }
        }break;

        case 1:{
            if(temp->buf[temp->tail] == 0x90){
                data[index++] = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                rcvPart = 2;
            }
            else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
            }
        }break;

        case 2:{
            if(temp->buf[temp->tail] == 0x00)
              {
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
             }
              else{

                data[index++] = temp->buf[temp->tail];
                length = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                rcvPart = 3;
               }
        }break;
        case 3:{
             data[index++] = temp->buf[temp->tail];
             temp->buf[temp->tail] = 0;
            if(index == length ){
                switch (data[3])
                {
                      case 0xA3:       NX_get_groundbase_cmd(data); break;       //地面站发送的控制指令
                      case 0xA4:       NX_get_groundbase_heart_beat(data);break;     ///地面站发送的heart_beat

                      case 0xB1:       NX_get_groundbase_waypoints_cmd(data); break;     //地面站发送的航点控制指令
                      case 0XB2:       NX_get_groundbase_single_waypoints(data);break;    //地面站发送的单条航点信息
                      case 0xB3:       user_get_flightcontrol_waypoints_cmd_feedback(data);break;

                      case 0xC0:       NX_get_groundbase_tracking_waypoints(data);break;

                    default: break;
                }
                rcvPart = 0;
                index = 0;
            }
        }break;

        default:
            break;
    }
  
}

void analyse_flight_control(circleBuf_t* temp, uint8_t* data)  //按协议解析飞控数据
{
    static uint8_t rcvPart = 0;
    static uint8_t index = 0;
    static uint8_t length = 0;

    switch (rcvPart)
    {
        case 0:{
            if(temp->buf[temp->tail] == 0x43){
                data[index++] = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                rcvPart = 1;
                // std::cout << "接  受  到  1  了" << std::endl;
            }
            else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
            }
        }break;

        case 1:{
            if(temp->buf[temp->tail] == 0x57){
                data[index++] = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                rcvPart = 2;
                // std::cout << "接  受  到  2  了" << std::endl;
            }
            else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
            }
        }break;

        case 2:{
            if(temp->buf[temp->tail] == 0x00) //帧长度
              {
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
             }
              else{
                data[index++] = temp->buf[temp->tail];
                length = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                rcvPart = 3;
                // std::cout << "接  受  到  3  了" << std::endl;
               }
        }break;
        case 3:{
            if(temp->buf[temp->tail] == 0x00) //地址
              {
                data[index++] = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                rcvPart = 4;
                // std::cout << "接  受  到  4  了" << std::endl;
             }
              else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
               }
        }break;
        case 4:{  
             data[index++] = temp->buf[temp->tail];
             temp->buf[temp->tail] = 0;

            if((index - 7)== length ){
                switch (data[4])          //包ID
                {
                      case 0XFF:        NX_get_flight_control_cmd_ACK(data,length); break;     //飞控控制包、航点上传包回传

                      case 0x08:         Query_to_flight_control_back_flightpoint(data,length);break;  //航点上传包回传

                      case 0X0A:        NX_get_flight_control_query1_back(data,length); break;  //飞控查询包1

                      case 0X0B:        NX_get_flight_control_query2_back(data,length);break;  //飞控查询包2

                      case 0X0C:        NX_get_flight_control_query3_back(data,length);break;  //飞控查询包3

                      case 0X0D:        NX_get_flight_control_query4_back(data,length);break;  //飞控查询包4

                      case 0X10:         NX_get_flight_control_query7_back(data,length);break;  //飞控查询包7

                      case 0X11:         NX_get_flight_control_query8_back(data,length);break;   //飞控查询包8

                    default: break;
                }
                rcvPart = 0;
                index = 0;
            }
        }break;

        default:
            break;
    }
}
float IEEE754Tofloat(uint8_t data1,uint8_t data2,uint8_t data3,uint8_t data4){
    uint32_t number = (data1<< 24 )|(data2<<16)|(data3<<8)|(data4);
    //printf("number:%x\n",((data1<< 24 )|(data2<<16)|(data3<<8)|(data4)));
    float v_float = 0 ;
    if(number != 0){
        int sign = number >> 31;//三目运算符
        if(sign == 1){
            sign = -1;
        }
        else sign = 1;
        //std::cout << "sign = "<<sign<< std::endl;
	    unsigned int exponent = ((number >> 23) & 0xff) - 127;//先右移操作，再按位与计算，出来结果是30到23位对应的e
        //std::cout << "exponent = "<<exponent << std::endl;
	    float mantissa = 1 + ((float)(number & 0x7fffff) / 0x7fffff);//将22~0转化为10进制，得到对应的x
        //std::cout << "mantissa = "<<mantissa << std::endl;
	    v_float = sign * mantissa * pow(2, exponent);
        //std::cout << "v_float = "<<v_float << std::endl;
    }
    return v_float;
}
/*
void analyse_camera_imformation(circleBuf_t* temp, uint8_t* data,uint8_t cnt)     
{
    static uint8_t rcvPart = 0;
    static uint8_t index = 0;
    static uint8_t length = 0;

    switch (rcvPart)
    {}
        
        case 0:{
            if(cnt == 48){
                rcvPart = 1;
                std::cout << "length correct" << std::endl;
            }
            
            else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
                let0();
            }
        }break;
        
        case 0:{
            if(temp->buf[temp->tail] == 0x09){
                data[index++] = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                //temp->tail++;
                rcvPart = 2;
                std::cout << "接  受  到  start  了" << std::endl;
            }
            else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
                let0();
            }
        }break;
        case 2:{
            if(temp->buf[temp->tail] == 0x09){
                data[index++] = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                //temp->tail++;
                rcvPart = 3;
                std::cout << "接  受  到  start  了" << std::endl;
            }
            else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
                let0();
            }
        }break;
        case 3:{
            if(temp->buf[temp->tail] == 0x09){
                data[index++] = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                rcvPart = 4;
                //temp->tail++;
                std::cout << "接  受  到  start  了" << std::endl;
            }
            else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
                let0();
            }
        }break;
        case 4:{
                uint8_t data[42];
                std::cout << "anliay" << std::endl;
                for(int i = 0 ; i < 42 ; i++){
                    data[i] = temp->buf[temp->tail] ;
                    temp->tail++;
                    //std::cout<<"data["<<i<<"] : "<< data[i]<<std::endl;
                    //printf("from tail: %d data[%d]:%x \n",temp->tail ,i,data[i]);
                    //float test = IEEE754Tofloat(0xc2,0xd2,0x6e,0x18);
                    //std::cout<<"-105.21502685546875 : "<< test <<std::endl;
                }
                
                for(int i = 0 ; i < 8 ;i++){
                    switch(data[5*i]){
                        case 0x00:{
                            //printf("%d:%x,%x,%x,%x\n",i,data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4]);
                            target_x = IEEE754Tofloat(data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4])/1e3;
                            //std::cout<<"target_x : "<< target_x <<std::endl;
                        }break;
                        case 0x01:{
                            //printf("%d:%x,%x,%x,%x\n",i,data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4]);
                            target_y = IEEE754Tofloat(data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4])/1e3;
                            //std::cout<<"target_y : "<< target_y <<std::endl;
                        }break;
                        case 0x02:{
                            //printf("%d:%x,%x,%x,%x\n",i,data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4]);
                            target_z = IEEE754Tofloat(data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4])/1e3;
                            //std::cout<<"target_z : "<< target_z <<std::endl;
                        }break;
                        case 0x03:{
                            //printf("%d:%x,%x,%x,%x\n",i,data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4]);
                            target_t = IEEE754Tofloat(data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4])/1e3;
                            //std::cout<<"target_t : "<< target_t <<std::endl;
                        }break;
                        case 0x04:{
                            //printf("%d:%x,%x,%x,%x\n",i,data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4]);
                            target_T = IEEE754Tofloat(data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4])/1e3;
                            //std::cout<<"target_T : "<< target_T <<std::endl;
                        }break;
                        case 0x05:{
                            //printf("%d:%x,%x,%x,%x\n",i,data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4]);
                            current_x = IEEE754Tofloat(data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4])/1e3;
                            //std::cout<<"current_x : "<< current_x <<std::endl;
                        }break;
                        case 0x06:{
                            //printf("%d:%x,%x,%x,%x\n",i,data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4]);
                            current_y = IEEE754Tofloat(data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4])/1e3;
                            //std::cout<<"current_y : "<< current_y <<std::endl;
                        }break;
                        case 0x07:{
                            //printf("%d:%x,%x,%x,%x\n",i,data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4]);
                            current_z = IEEE754Tofloat(data[5*i+1],data[5*i+2],data[5*i+3],data[5*i+4])/1e3;
                            //std::cout<<"current_z : "<< current_z <<std::endl;
                        }break;
                        case 0x08:{
                            dorp_cmd = data[41];
                            std::cout<<"dorp_cmd : "<< dorp_cmd <<std::endl;
                        }break;
                    }
                }
                
                rcvPart = 5;               
                }break;
            case 5:{
            if(temp->buf[temp->tail] == 0x10){
                data[index++] = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                //temp->tail++;
                rcvPart = 6;
                std::cout << "接  受  到  start  了" << std::endl;
            }
            else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
                let0();
            }
            }break;
            case 6:{
                if(temp->buf[temp->tail] == 0x10){
                    data[index++] = temp->buf[temp->tail];
                    temp->buf[temp->tail] = 0;
                    //temp->tail++;
                    rcvPart = 7;
                    std::cout << "接  受  到  start  了" << std::endl;
                }
                else{
                    data[index++] = 0;
                    rcvPart = 0;
                    index = 0;
                    let0();
                }
            }break;
            case 7:{
                if(temp->buf[temp->tail] == 0x10){
                    data[index++] = temp->buf[temp->tail];
                    temp->buf[temp->tail] = 0;
                    rcvPart = 8;
                    //temp->tail++;
                    std::cout << "接  受  到  start  了" << std::endl;
                }
                else{
                    data[index++] = 0;
                    rcvPart = 0;
                    index = 0;
                    let0();
                }
            }break;
    }
}*/

uint8_t NX_get_flight_control_cmd_ACK(uint8_t* data,uint8_t len)      //控制指令回传
{
    static uint8_t i = 0;
    uint8_t recv_data[len + 7] = {0};
    for(i = 0;i < (len+7);i++)
    {
        recv_data[i] = data[i];
        //printf("recv_data[%d] = %x\n",i,recv_data[i]);
    }

    if(recv_data[5] == 0x01)
    {
        if(CalculateCrc16(recv_data,len+7) == 0)       //CRC校验
        {

            switch (recv_data[6])
            {
                case 0x0D:

                    break;
                case 0X0E:
                    break;
                case 0X20:
                    if(groundbase_cmd.control_cmd == 0x00) 
                    {
                        usr_flag.control_flag = true;   
                        //std::cout << " ～～～～～ 控 制 包 数 据 发 送 成 功 ～～～～～" << std::endl;
                    }
                    else
                    {
                        usr_flag.control_flag = false;   
                        // std::cout << " ！！！！！控 制 包 数 据 发 送 失 败！！！！！" << std::endl;
                    }
                    break;
                case 0X22:
                 break;
                case 0X23:
                    if((groundbase_cmd.control_cmd == 0x02)||(groundbase_cmd.control_cmd == 0x03)||(groundbase_cmd.control_cmd == 0x04)||(groundbase_cmd.control_cmd == 0x05)
                        ||(groundbase_cmd.control_cmd == 0x16)||(groundbase_cmd.control_cmd == 0x17)||(groundbase_cmd.control_cmd == 0x18)||(groundbase_cmd.control_cmd == 0x19)
                        ||(groundbase_cmd.control_cmd == 0x06)||(groundbase_cmd.control_cmd == 0x10))
                    {

   
                        usr_flag.control_flag = true;   
                        //std::cout << " ～～～～～ 控 制 包 数 据 发 送 成 功 ～～～～～" << std::endl;
                    }
                    else
                    {
                        usr_flag.control_flag = false;   
                         //std::cout << " ！！！！！控 制 包 数 据 发 送 失 败！！！！！" << std::endl;
                    }
                    break;
                case 0X40:
                    if(groundbase_cmd.control_cmd == 0x02)
                    {
                        usr_flag.control_flag = true;   
                        //std::cout << " ～～～～～ 控 制 包 数 据 发 送 成 功 ～～～～～" << std::endl;
                    }
                    else
                    {
                        usr_flag.control_flag = false;   
                         //std::cout << " ！！！！！控 制 包 数 据 发 送 失 败！！！！！" << std::endl;
                    }
                    break;
                case 0X41:
                    if(groundbase_cmd.control_cmd == 0x03)
                    {
                        usr_flag.control_flag = true;   
                        //std::cout << " ～～～～～ 控 制 包 数 据 发 送 成 功 ～～～～～" << std::endl;
                    }
                    else
                    {
                        usr_flag.control_flag = false;   
                         //std::cout << " ！！！！！控 制 包 数 据 发 送 失 败！！！！！" << std::endl;
                    }
                     break;
                case 0X42:
                    if(groundbase_cmd.control_cmd == 0x07)
                    {
                        usr_flag.control_flag = true;   
                        //std::cout << " ～～～～～ 控 制 包 数 据 发 送 成 功 ～～～～～" << std::endl;
                    }
                    else
                    {
                        usr_flag.control_flag = false;   
                        //std::cout << " ！！！！！控 制 包 数 据 发 送 失 败！！！！！" << std::endl;
                    }
                    break;
                case 0X43:
                    if(groundbase_cmd.control_cmd == 0x08)
                    {
                        usr_flag.control_flag = true;   
                        //std::cout << " ～～～～～ 控 制 包 数 据 发 送 成 功 ～～～～～" << std::endl;
                    }
                    else
                    {
                        usr_flag.control_flag = false;   
                        //std::cout << " ！！！！！控 制 包 数 据 发 送 失 败！！！！！" << std::endl;
                    }
                    break;
                case 0x44:
                    if(groundbase_cmd.control_cmd == 0x04)
                    {
                        usr_flag.control_flag = true;   
                        //std::cout << " ～～～～～ 控 制 包 数 据 发 送 成 功 ～～～～～" << std::endl;
                    }
                    else
                    {
                        usr_flag.control_flag = false;   
                         //std::cout << " ！！！！！控 制 包 数 据 发 送 失 败！！！！！" << std::endl;
                    }
                    break;
                default:
                    break;
                }

        }
        else
        {
            usr_flag.control_flag = false;  
            std::cout << " ！！！！！控 制 包 数 据 回 传 校 验 失 败！！！！！" << std::endl;
        }

    }
    else if(data[5] == 0x08)           //当前航点上传
    {
        if(CalculateCrc16(recv_data,len+7) == 0)       //CRC校验
        {
            if(((recv_data[8]<<8 & 0xFF00)|recv_data[7]) == groundbase_waypoint.current_index)    //航点查询校验
            {
                for(i = 0;i < (len+7);i++)
                {
                    printf("recv_data[%d] = %x\n",i,recv_data[i]);
                }
                usr_flag.flight_point_flag = true;   
                 std::cout << " ～～～～～ 航 点 数 据 发 送 成 功,shoudaofankui ～～～～～" << std::endl;
                 printf("usr_flag.flight_point_flag = %d\n",usr_flag.flight_point_flag);
                 printf("usr_flag.waypoints_single_flag = %d\n",waypoints_single_flag);

                        
            }
            else
            {
                usr_flag.flight_point_flag = false;   
                std::cout << " ！！！！！航 点 数 据 发 送 失 败！！！！！" << std::endl;
            }
        }
        else
        {
            usr_flag.flight_point_flag = false;  
            std::cout << " ！！！！！航 点 查 询 回 传 校 验 失 败！！！！！" << std::endl;
        }
    }
    return (data[2]+7);
}

uint8_t Query_to_flight_control_back_flightpoint(uint8_t* data,uint8_t len)
{
  static uint8_t i = 0;
    uint8_t recv_data[len + 7] = {0};
    for(i = 0;i < (len+7);i++)
    {
        recv_data[i] = data[i];
        printf("hangdianhuichuang   recv_data[%x] = %d\n",i,recv_data[i]);
    }
    if(0 == CalculateCrc16(recv_data,len+7)) ///crc校验
    {
    ///将飞机上传的航点信息保留在飞控航点的结构体里
    flight_control_waypoint.point_number = ((recv_data[6]<<8)&0xFF00)|(recv_data[5]&0XFF);  ///航点总数
     printf("flight_control_waypoint.point_number = %d\n",flight_control_waypoint.point_number);
    flight_control_waypoint.current_index = ((recv_data[8]<<8)&0xFF00)|(recv_data[7]&0XFF); ///当前航点序号
    flight_control_waypoint.next_index = flight_control_waypoint.current_index+1;           ///下一个航点序号

     flight_control_waypoint.point_i_lon = ((recv_data[13]<<8)&0xFF00)|(recv_data[12]&0XFF);      ///航点经度
     flight_control_waypoint.point_f_lon = ((recv_data[17]<<24)&0xFF000000)|(((recv_data[16]<<16)&0XFF0000))|((recv_data[15]<<8)&0xFF00)|(recv_data[14]&0xFF);

     flight_control_waypoint.point_i_lat = ((recv_data[19]<<8)&0xFF00)|(recv_data[18]&0XFF);     ///航点纬度
     flight_control_waypoint.point_f_lat = ((recv_data[23]<<24)&0xFF000000)|(((recv_data[22]<<16)&0XFF0000))|((recv_data[21]<<8)&0xFF00)|(recv_data[20]&0xFF);
                                                                                          ///航点高度
     flight_control_waypoint.point_altitude = ((recv_data[27]<<24)&0xFF000000)|(((recv_data[26]<<16)&0XFF0000))|((recv_data[25]<<8)&0xFF00)|(recv_data[24]&0xFF);

    if(recv_data[34] == 2){                                   ///航点模式   
      flight_control_waypoint.point_mode = 2;
    } else if(recv_data[34] == 4)  {
       flight_control_waypoint.point_mode = 4;
    }  else if(recv_data[34] == 7){
         flight_control_waypoint.point_mode = 3;
    }else if(recv_data[34] == 8){
         flight_control_waypoint.point_mode = 5;
    }           
                                ///航点模式   
     flight_control_waypoint.point_duration = ((recv_data[36]<<8)&0xFF00)|(recv_data[35]&0XFF);   ///持续时间       

      NX_get_up_waypoint_mess_flag = true;
      NX_get_up_waypoint_single_flag =false;
    }
    return 0;
}

uint8_t NX_get_flight_control_query1_back(uint8_t* data,uint8_t len)      //查询回传包1
{
    static uint8_t i = 0;
    uint8_t recv_data[len + 7] = {0};
    for(i = 0;i < (len+7);i++)
    {
        recv_data[i] = data[i];
    }

        if(0 == CalculateCrc16(recv_data,len+7))       //CRC校验
        {
            flight_control_to_NX_data.gps_num = data[5];

            flight_control_to_NX_data.ahrss_state = data[6];

            flight_control_to_NX_data.i_lon = ((data[8]<<8)&0xFF00)|(data[7]&0XFF);
            flight_control_to_NX_data.f_lon = ((data[12]<<24)&0xFF000000)|(((data[11]<<16)&0XFF0000))|((data[10]<<8)&0xFF00)|(data[9]&0xFF);

            flight_control_to_NX_data.i_lat = ((data[14]<<8)&0xFF00)|(data[13]&0XFF);
            flight_control_to_NX_data.f_lat = ((data[18]<<24)&0xFF000000)|(((data[17]<<16)&0XFF0000))|((data[16]<<8)&0xFF00)|(data[15]&0xFF);

            flight_control_to_NX_data.alt = ((data[22]<<24)&0xFF000000)|(((data[21]<<16)&0XFF0000))|((data[20]<<8)&0xFF00)|(data[19]&0xFF);

            flight_control_to_NX_data.vn = ((data[24]<<8)&0xFF00)|(data[23]&0XFF);

            flight_control_to_NX_data.ve = ((data[26]<<8)&0xFF00)|(data[25]&0XFF);

            flight_control_to_NX_data.vu = ((data[28]<<8)&0xFF00)|(data[27]&0XFF);

            flight_control_to_NX_data.phi = ((data[30]<<8)&0xFF00)|(data[29]&0XFF);

            flight_control_to_NX_data.the = ((data[32]<<8)&0xFF00)|(data[31]&0XFF);

            flight_control_to_NX_data.psi = ((data[34]<<8)&0xFF00)|(data[33]&0XFF);

            usr_flag.query_1 = true;   
             //std::cout << " ～～～～～ 查 询 包 1 数 据 接 收 成 功 ～～～～～" << std::endl;
             if(flight_control_to_NX_data.ahrss_state == 0x00)
             {
                 drone_data_to_groundbase.fix_status = 0;
             }
             else if(flight_control_to_NX_data.ahrss_state == 0x02)
             {
                 drone_data_to_groundbase.fix_status = 1; // GPS
             }
             else if(flight_control_to_NX_data.ahrss_state == 0x04)
             {
                //  if( landpad_rtk_status == true)
                //  {
                //         drone_data_to_groundbase.fix_status = 5; //drone RTK and landpad RTK
                //  }
                //  else
                //  {
                        drone_data_to_groundbase.fix_status = 2; //RTK
                //  } 
             }
             else if(flight_control_to_NX_data.ahrss_state == 0x03)
             {
                 drone_data_to_groundbase.fix_status = 3;// DGPS
             }
             else
             {
                 drone_data_to_groundbase.fix_status = 0;
             }


             drone_data_to_groundbase.roll = flight_control_to_NX_data.phi&0xFFFF;
             drone_data_to_groundbase.pitch = flight_control_to_NX_data.the&0xFFFF;
             drone_data_to_groundbase.yaw = flight_control_to_NX_data.psi&0xFFFF;


               int16_t drone_data_vel_x = (int16_t)((float)((1.0f)*flight_control_to_NX_data.vn/10));    //beixiang
               int16_t drone_data_vel_y = (int16_t)((float)((1.0f)*flight_control_to_NX_data.ve/10));
                   
                

             drone_data_to_groundbase.vel.z = (int16_t)((float)((1.0f)*flight_control_to_NX_data.vu/10));

           drone_data_to_groundbase.vel.x = (int16_t)(sqrtf((drone_data_vel_x *drone_data_vel_x )+(drone_data_vel_y*drone_data_vel_y)));

             drone_data_to_groundbase.curr_pos.lat = (int32_t)(flight_control_to_NX_data.i_lat*10000000 +(int32_t)(flight_control_to_NX_data.f_lat/100));
             drone_data_to_groundbase.curr_pos.lng = (int32_t)(flight_control_to_NX_data.i_lon*10000000 + (int32_t)(flight_control_to_NX_data.f_lon/100));
             drone_data_to_groundbase.curr_pos.alt = (int32_t)((flight_control_to_NX_data.alt - flight_control_to_NX_data.home_alt)/10);    //飞控数据单位为mm    地面站需要cm
        }
        else
        {
            usr_flag.query_1 = false;   
            std::cout << " ！！！！！查  询  包  1 校 验 失 败！！！！！" << std::endl;
        }
        return 0;

}

uint8_t NX_get_flight_control_query2_back(uint8_t* data,uint8_t len)      //查询回传包2
{
    static uint8_t i = 0;
    uint8_t recv_data[len + 7] = {0};
    for(i = 0;i < (len+7);i++)
    {
        recv_data[i] = data[i];
    }

        if(0 == CalculateCrc16(recv_data,len+7))       //CRC校验
        {
            flight_control_to_NX_data.gyr_x = ((data[6]<<8)&0xFF00)|(data[5]&0XFF);
            flight_control_to_NX_data.gyr_y = ((data[8]<<8)&0xFF00)|(data[7]&0XFF);
            flight_control_to_NX_data.gyr_z = ((data[10]<<8)&0xFF00)|(data[9]&0XFF);

            flight_control_to_NX_data.acc_x = ((data[12]<<8)&0xFF00)|(data[11]&0XFF);
            flight_control_to_NX_data.acc_y = ((data[14]<<8)&0xFF00)|(data[13]&0XFF);
            flight_control_to_NX_data.acc_z = ((data[16]<<8)&0xFF00)|(data[15]&0XFF);

            flight_control_to_NX_data.mag_x = ((data[18]<<8)&0xFF00)|(data[17]&0XFF);
            flight_control_to_NX_data.mag_y = ((data[20]<<8)&0xFF00)|(data[19]&0XFF);
            flight_control_to_NX_data.mag_z = ((data[22]<<8)&0xFF00)|(data[21]&0XFF);


            usr_flag.query_2 = true;  
            //std::cout << " ～～～～～ 查 询 包 2 数 据 接 收 成 功 ～～～～～" << std::endl;
        }
        else
        {
            usr_flag.query_2 = false;  
            std::cout << " ！！！！！查  询  包  2 校 验 失 败！！！！！" << std::endl;
        }
        return 0;

}

uint8_t NX_get_flight_control_query3_back(uint8_t* data,uint8_t len)     //查询回传包3
{
    static uint8_t i = 0;
    uint8_t recv_data[len + 7] = {0};
    for(i = 0;i < (len+7);i++)
    {
        recv_data[i] = data[i];
    }

        if(0 == CalculateCrc16(recv_data,len+7))       //CRC校验
        {
            flight_control_to_NX_data.home_lon_lat_valid = data[5];
            
            flight_control_to_NX_data.home_i_lon = ((data[7]<<8)&0xFF00)|(data[6]&0XFF);
            flight_control_to_NX_data.home_f_lon = ((data[11]<<24)&0xFF000000)|(((data[10]<<16)&0XFF0000))|((data[9]<<8)&0xFF00)|(data[8]&0xFF);

           // printf("f flight_control_to_NX_data.home_i_lon = %d\n", flight_control_to_NX_data.home_i_lon);
           //  printf(" flight_control_to_NX_data.home_f_lon= %d\n",  flight_control_to_NX_data.home_f_lon);

            flight_control_to_NX_data.home_i_lat = ((data[13]<<8)&0xFF00)|(data[12]&0XFF);
            flight_control_to_NX_data.home_f_lat = ((data[17]<<24)&0xFF000000)|(((data[16]<<16)&0XFF0000))|((data[15]<<8)&0xFF00)|(data[14]&0xFF);

            flight_control_to_NX_data.home_alt = ((data[21]<<24)&0xFF000000)|(((data[20]<<16)&0XFF0000))|((data[19]<<8)&0xFF00)|(data[18]&0xFF);

            flight_control_to_NX_data.battery_vpower = ((data[43]<<8)&0xFF00)|(data[42]&0XFF);
          //  printf("flight_control_to_NX_data.battery_vpower = %d\n",flight_control_to_NX_data.battery_vpower);

            usr_flag.query_3 = true;  
            //std::cout << " ～～～～～ 查 询 包 3 数 据 接 收 成 功 ～～～～～" << std::endl;

            drone_data_to_groundbase.battery = (flight_control_to_NX_data.battery_vpower) * 0.1;
        }
        else
        {
            usr_flag.query_3 = false;  
            std::cout << " ！！！！！查  询  包  3 校 验 失 败！！！！！" << std::endl;
        }

    return 0;

}

uint8_t NX_get_flight_control_query4_back(uint8_t* data,uint8_t len)     //查询回传包4
{
    static uint8_t i = 0;
    uint8_t recv_data[len + 7] = {0};
    for(i = 0;i < (len+7);i++)
    {
        recv_data[i] = data[i];
    }

        if(0 == CalculateCrc16(recv_data,len+7))       //CRC校验
        {
            flight_control_to_NX_data.firmware_subver = data[5];
            flight_control_to_NX_data.firmware_ver = data[6];

            flight_control_to_NX_data.fly_mileage = ((data[54]<<24)&0xFF000000)|(((data[53]<<16)&0XFF0000))|((data[52]<<8)&0xFF00)|(data[51]&0xFF);

            flight_control_to_NX_data.fly_sorties = ((data[58]<<24)&0xFF000000)|(((data[57]<<16)&0XFF0000))|((data[56]<<8)&0xFF00)|(data[55]&0xFF);

            flight_control_to_NX_data.fly_time = ((data[62]<<24)&0xFF000000)|(((data[61]<<16)&0XFF0000))|((data[60]<<8)&0xFF00)|(data[59]&0xFF);

            usr_flag.query_4 = true;  
           // std::cout << " ～～～～～ 查 询 包 4 数 据 接 收 成 功 ～～～～～" << std::endl;

        }
        else
        {
            usr_flag.query_4 = false;  
            std::cout << " ！！！！！查  询  包  4 校 验 失 败！！！！！" << std::endl;
        }
    return 0;

}

uint8_t NX_get_flight_control_query7_back(uint8_t* data,uint8_t len)      //查询回传包7
{
    static uint8_t i = 0;
    uint8_t recv_data[len + 7] = {0};
    for(i = 0;i < (len+7);i++)
    {
        recv_data[i] = data[i];
    }

        if(0 == CalculateCrc16(recv_data,len+7))       //CRC校验
        {
            flight_control_to_NX_data.aircraft_power_motor_status = data[7];

            flight_control_to_NX_data.takeoff_landing_on_ground_flag = data[8];

            flight_control_to_NX_data.flight_mode = data[9];

            flight_control_to_NX_data.navi_sub_mode = (data[12]&0x0F);

            usr_flag.query_7 = true;  
            //std::cout << " ～～～～～ 查 询 包 7 数 据 接 收 成 功 ～～～～～" << std::endl;
            if(flight_control_to_NX_data.takeoff_landing_on_ground_flag == 1)   //自动起飞
            {
                drone_data_to_groundbase.flight_mode = 8;                                 //起飞
            }
            else if(flight_control_to_NX_data.takeoff_landing_on_ground_flag == 2)   //自动降落
            {
                drone_data_to_groundbase.flight_mode = 7;                                 //降落
            }
            else
            {
                //std::cout << " 111111111111111111111111111111111111111111111111" << std::endl;
                if(flight_control_to_NX_data.flight_mode == 5)//主导航模式
                {
                    //printf("flight_control_to_NX_data.flight_mode%d\n",flight_control_to_NX_data.flight_mode);
                    //printf("flight_control_to_NX_data.navi_sub_mode%d\n",flight_control_to_NX_data.navi_sub_mode);
                    if(flight_control_to_NX_data.navi_sub_mode == 3 || usr_flag.remote_lose_rc_link_flag)
                    {
                         drone_data_to_groundbase.flight_mode = 6;           //返航
                          //std::cout << " 6666666666666666666666666" << std::endl;
                    }
                    else if(flight_control_to_NX_data.navi_sub_mode == 5)
                    {
                        drone_data_to_groundbase.flight_mode = 5;          //多点导航（自动执行任务）
                        //std::cout << " 5555555555555555555555555555555" << std::endl;
                    }
                        
             
                }
                else if(flight_control_to_NX_data.flight_mode == 4)    //GPS模式
                {

                    
                }
            }


            if((flight_control_to_NX_data.aircraft_power_motor_status&0x10) == 0)
            {
                drone_data_to_groundbase.flight_status = 0;
            }
            else if((flight_control_to_NX_data.aircraft_power_motor_status&0x10) == 0x10)
            {
                drone_data_to_groundbase.flight_status = 1;
                //std::cout << " 1111111111111111111111111111111" << std::endl;
            }
                

            // printf("flight_control_to_NX_data.aircraft_power_motor_status = %d\n",flight_control_to_NX_data.aircraft_power_motor_status);
            // printf("flight_control_to_NX_data.aircraft_power_motor_status&0x10 = %d\n",flight_control_to_NX_data.aircraft_power_motor_status&0x10);



            // printf("flight_control_to_NX_data.takeoff_landing_on_ground_flag%d\n",flight_control_to_NX_data.takeoff_landing_on_ground_flag);
            
            // printf("flight_control_to_NX_data.navi_sub_mode%d\n",flight_control_to_NX_data.navi_sub_mode);


            

        }
        else
        {
            usr_flag.query_7 = false;  
            std::cout << " ！！！！！查  询  包  7 校 验 失 败！！！！！" << std::endl;
        }
    return 0;

}


uint8_t NX_get_flight_control_query8_back(uint8_t* data,uint8_t len)      //查询回传包8
{
    static uint8_t i = 0;
    uint8_t recv_data[len + 7] = {0};
    for(i = 0;i < (len+7);i++)
    {
        recv_data[i] = data[i];
    }

        if(0 == CalculateCrc16(recv_data,len+7))       //CRC校验
        {
            flight_control_to_NX_data.breakdown_flag = data[5];

            flight_control_to_NX_data.limit_lost_fail_flag = data[6];

            flight_control_to_NX_data.low_fail_flag = data[7];

            flight_control_to_NX_data.smart_battery_status = ((data[13]<<8)&0xFF00)|(data[12]&0XFF);

           // printf("flight_control_to_NX_data.breakdown_flag = %d\n",flight_control_to_NX_data.breakdown_flag);
           // printf("flight_control_to_NX_data.limit_lost_fail_flag = %d\n", flight_control_to_NX_data.limit_lost_fail_flag);
            //printf("flight_control_to_NX_data.low_fail_flag = %d\n", flight_control_to_NX_data.low_fail_flag);
           // printf("flight_control_to_NX_data.smart_battery_status = %d\n", flight_control_to_NX_data.smart_battery_status);

            usr_flag.query_8 = true;  
            //std::cout << " ～～～～～ 查 询 包 8 数 据 接 收 成 功 ～～～～～" << std::endl;
            if((flight_control_to_NX_data.breakdown_flag == 0x00)&&((flight_control_to_NX_data.limit_lost_fail_flag ==0x00)||(flight_control_to_NX_data.limit_lost_fail_flag == 0x80))&&((flight_control_to_NX_data.low_fail_flag==0x00)||(flight_control_to_NX_data.low_fail_flag == 0x8C)||
                (flight_control_to_NX_data.low_fail_flag == 0x0C))&&(flight_control_to_NX_data.smart_battery_status == 0x0000))   //zhengchang
                usr_flag.self_check_flag = true;
            else
                usr_flag.self_check_flag = false;

            // printf("usr_flag.self_check_flag = %d\n",usr_flag.self_check_flag);

            if(usr_flag.self_check_flag == true)
                drone_data_to_groundbase.selfcheck = 0x01;                       //通过
            else if(usr_flag.self_check_flag == false)
                drone_data_to_groundbase.selfcheck = 0x02;                //不通过
        }
        else
        {
            usr_flag.query_8 = false;  
            std::cout << " ！！！！！查  询  包  8 校 验 失 败！！！！！" << std::endl;
        }
    return 0;
}

void analyse_SBUS(circleBuf_t* temp, uint8_t* data)    //按协议解析遥控器SBUS数据
{
    static uint8_t rcvPart = 0;
    static uint8_t index = 0;
    //static uint8_t length = 0;

    switch (rcvPart)
    {
        case 0:{
            if(temp->buf[temp->tail] == 0x0F){
                data[index++] = temp->buf[temp->tail];
                temp->buf[temp->tail] = 0;
                rcvPart = 1;
                 //std::cout << " ！！！！！S     B     U     S！！！！！" << std::endl;
            }
            else{
                data[index++] = 0;
                rcvPart = 0;
                index = 0;
            }
        }break;

        case 1:{  
             data[index++] = temp->buf[temp->tail];
             temp->buf[temp->tail] = 0;

            // if(index == 25)
            // {
            //     if(data[24] == 0x00)
            //     {
            //         SBUS_data_transform_to_remote_channels_value(data, 22);
            //     }
            //     rcvPart = 0;
            //     index = 0;
            // }

            if(index == 35)
            {
                SBUS_data_transform_to_remote_channels_value(data, 35);
                rcvPart = 0;
                index = 0;
            }
        }break;

        default:
            break;
    }
}


  uint8_t SBUS_data_transform_to_remote_channels_value(uint8_t* data,uint8_t len)  //SBUS值转化为具体遥控器控制杆通道值
  {
        static uint8_t buffer[22] = {0};
        static uint8_t i = 0;
        uint8_t recv_data[len] = {0};
        uint8_t BCC = 0;
        for(i = 0;i < len;i++)
        {
            recv_data[i] = data[i];
        }

        BCC = get_BCC(recv_data,len-1);

        if(BCC ==recv_data[34])
        {
            _channels[0] = (recv_data[1]<<8)|recv_data[2];
            _channels[1] = (recv_data[3]<<8)|recv_data[4];
            _channels[2] = (recv_data[5]<<8)|recv_data[6];
            _channels[3] = (recv_data[7]<<8)|recv_data[8];
            _channels[4] = (recv_data[9]<<8)|recv_data[10];
            _channels[5] = (recv_data[11]<<8)|recv_data[12];
            _channels[6] = (recv_data[13]<<8)|recv_data[14];
            _channels[7] = (recv_data[15]<<8)|recv_data[16];
            _channels[8] = (recv_data[17]<<8)|recv_data[18];
            _channels[9] = (recv_data[19]<<8)|recv_data[20];
            _channels[10] = (recv_data[21]<<8)|recv_data[22];
            _channels[11] = (recv_data[23]<<8)|recv_data[24];
            _channels[12] = (recv_data[25]<<8)|recv_data[26];
            _channels[13] = (recv_data[27]<<8)|recv_data[28];
            _channels[14] = (recv_data[29]<<8)|recv_data[30];
            _channels[15] = (recv_data[31]<<8)|recv_data[32];


            remote_cmd.UAV_acc = _channels[2];
            remote_cmd.UAV_yaw = _channels[3];
            remote_cmd.UAV_pitch = _channels[1];
            remote_cmd.UAV_roll = _channels[0];
              remote_cmd.UAV_land =   _channels[5];



           if( (_channels[2] <  400) && (_channels[3] <400 ) && ((_channels[1] <400 ) &&(_channels[0] <400 ) ) )
           {
               if(usr_flag.remote_control_flag)
               {
                   printf(" lose control33333333333333333333333333333= %d\n",usr_flag.remote_control_flag);
                    usr_flag.remote_lose_rc_link_flag = true;  ///losecontrol


                    //  reset_onboard_target();
                     printf("00000000000000000000000000000\n");
               }
             
              
           }
           if(drone_data_to_groundbase.flight_status == 0)
           {
              usr_flag.remote_lose_rc_link_flag = false;

           }


        if( !(usr_flag.remote_lose_rc_link_flag))
        {
            if(_channels[6] <1000)   ///    >1000:remote control    <1000:gcs
            {
              usr_flag.remote_control_flag = false;              ///gcs control
            }
            else
            { 
            // reset_onboard_target();   
 
            usr_flag.remote_control_flag = true;               ///remote  control

              printf(" remote control33333333333333333333333333333= %d\n",usr_flag.remote_control_flag);
            }
        }


          
            
        }

        return 0;

  }

uint8_t NX_serial_send_to_groundbase_10hz(LinuxSerialDevice* radio, drone_data_to_groundbase_t* para)   //A1大类：反馈无人机当前状态、模式、电量、吊仓状态
{
    uint8_t index = 0;
    const uint8_t length = 37;
    uint8_t buffer[length] = {0};
    uint8_t temp8 = 0;
    uint16_t temp16 = 0;
    index = set_head(buffer,index,0xeb,0x90,length,0xA1,usr_flag.flight_ID);   
    index = set_low_freq_onboard_to_ground_payload(buffer,index,para->selfcheck,
                        para->flight_mode,para->flight_status,para->fix_status,para->battery);
   
   //油门杆值
    temp16 =  remote_cmd.UAV_acc;
    buffer[index++] = USR_BYTE1(temp16);
    buffer[index++] = USR_BYTE0(temp16);
   
    //中心x坐标
    temp16 = current_x * 100;
    //printf("current_x : %d \n" , temp16);
    buffer[index++] = USR_BYTE1(temp16);
    buffer[index++] = USR_BYTE0(temp16);

    //中心y坐标
    temp16 = current_y * 100;
    buffer[index++] = USR_BYTE1(temp16);
    buffer[index++] = USR_BYTE0(temp16);

    //中心z坐标
    temp16 = current_z * 100;
    buffer[index++] = USR_BYTE1(temp16);
    buffer[index++] = USR_BYTE0(temp16);

    //时间t
    temp16 = current_time * 100;
    buffer[index++] = USR_BYTE1(temp16);
    buffer[index++] = USR_BYTE0(temp16);

    //投放点x坐标
    temp16 = target_x * 100;
    buffer[index++] = USR_BYTE1(temp16);
    buffer[index++] = USR_BYTE0(temp16);

    //投放点y坐标
    temp16 = target_y * 100;
    buffer[index++] = USR_BYTE1(temp16);
    buffer[index++] = USR_BYTE0(temp16);

    //识别到桅杆
    temp8 = IsIdentify;
    printf("IsIdentify : %d \n" , temp8);
    buffer[index++] = (uint8_t)temp8;

    //无人机被控状态
    temp8 = usr_flag.remote_control_flag;
    buffer[index++] = (uint8_t)temp8;

    //投放点z坐标
    temp16 = target_z * 100;
    buffer[index++] = USR_BYTE1(temp16);
    buffer[index++] = USR_BYTE0(temp16);

    //投放相位time
    temp16 = target_time * 100;
    buffer[index++] = USR_BYTE1(temp16);
    buffer[index++] = USR_BYTE0(temp16);

    //周期
    temp16 = target_T * 100;
    buffer[index++] = USR_BYTE1(temp16);
    buffer[index++] = USR_BYTE0(temp16);

    //无人机任务状态
    temp8 = mession_state;
    buffer[index++] = (uint8_t)temp8;

    //电磁铁状态反馈
    temp8 = electromagnet_state;
    buffer[index++] = (uint8_t)temp8;

    //子机状态反馈
    temp8 = son_plane_state;
    buffer[index++] = (uint8_t)temp8;

    index = set_checksum(buffer, index);

    // radio->send(buffer, index);
    UDP_send(buffer,UDP_fd,index);
    //std::cout << "11111111111111111发送";
    return index;

}


uint8_t NX_serial_send_to_groundbase_20hz(LinuxSerialDevice* radio, drone_data_to_groundbase_t* para)   //A2大类：反馈无人机姿态数据
{
    uint8_t index = 0;
    const uint8_t length = 29;
    uint8_t buffer[length] = {0};
    uint16_t curr_vel_xy;

    index = set_head(buffer,index,0xeb,0x90,length,0xA2,usr_flag.flight_ID);
    index = set_high_freq_onboard_to_ground_payload(buffer,index,para->roll,para->pitch,para->yaw,
                            para->vel.x,para->vel.z,para->curr_pos.lat,para->curr_pos.lng,para->curr_pos.alt);
    index = set_checksum(buffer, index);
 
    // radio->send(buffer, index);
    UDP_send(buffer,UDP_fd,index);

    return index;
}

// uint8_t NX_serial_send_to_flight_control(LinuxSerialDevice* radio, groundbase_cmd_t* para)      //NX向飞控发送控制包
// {
//     static uint16_t crc;
//     static uint8_t index = 20;
//     static uint8_t send_data[20] = {0};
//     int16_t GPS_control =0;
//     uint8_t last_groundbase_cmd = 0;
//     static uint8_t i = 0;

//     static uint8_t sum_send_time = 0;

//     send_data[0] = 0x43;
//     send_data[1] = 0x57;
//     send_data[2] = 13;
//     send_data[3] = 0x00;
//     send_data[4] = 0x01;
//     if((usr_flag.NX_wait_groundbase_controlcmd_ready_flag == 3)&&(usr_flag.control_flag == true))
//     {
//         if((groundbase_cmd.control_cmd ==0x16) || 
//                 (groundbase_cmd.control_cmd ==0x17) ||
//                     (groundbase_cmd.control_cmd ==0x18)||
//                         (groundbase_cmd.control_cmd ==0x19)||
//                             (groundbase_cmd.control_cmd ==0x10 )){
//         }
       
//         else{
//          groundbase_cmd.control_cmd = 1;
     
//         }

//     }

//     if( (usr_flag.remote_lose_rc_link_flag) && (usr_flag.remote_control_flag) )
//     {
//          send_data[5] = 0x44;
//          //roll
//           send_data[6] = 0;
//           send_data[7] = 0;
//         //pitch
//           send_data[8] = 0;
//           send_data[9] = 0;
//         //throttle
//           send_data[10] = 0;
//           send_data[11] = 0;
//           //yaw
//           send_data[12] = 0;
//           send_data[13] = 0;

//     }else{
       
//    if((!usr_flag.remote_control_flag))       ///gcs control     //遥控器没动  
//     {

//         if(flight_control_lose_massage_flag)
//         {
//              para -> control_cmd = 0x04;   
//             //   printf("+++++++++++++++++++++++jjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjj++++++++\n");
//         }

//         if(onboard_land_state == Landed){
//             para -> control_cmd = 0x27;   
//         }



//             if((usr_flag.NX_get_groundbase_heartbeat_cnt >= 3) && (usr_flag.gcs_lose_heart_beat_flag))   ///lose gcs heartbeat
//             {
//                 para -> control_cmd = 0x04;   
//                 reset_onboard_target();

//                 usr_flag.NX_get_groundbase_heartbeat_cnt = 0;
//             }

//             // if(usr_flag.NX_get_groundbase_heartbeat_cnt >= 3){
//             //        usr_flag.NX_get_groundbase_heartbeat_cnt = 0;
//             // }
       
       
//             switch (para -> control_cmd)
//             {
//                 case 0x00://解锁
//                     send_data[5] = 0x20;
//                     break;// 起浆
//                 case 0x01://加锁？？？？？？？
//                     send_data[5] = 0x00;
//                     //roll
//                      send_data[6] = 0;
//                      send_data[7] = 0;
//                     //pitch
//                     send_data[8] = 0;
//                     send_data[9] = 0;
//                     //throttle
//                     send_data[10] = 0;
//                     send_data[11] = 0;
//                     //yaw
//                     send_data[12] = 0;
//                     send_data[13] = 0;
//                     break;   //无指令，空
//                 case 0x02://起飞
//                     send_data[5] = 0x40;
//                     break;   //自动起飞
//                 case 0x03://降落
//                     send_data[5] = 0x41;
//                     break;  //自动降落
//                     // printf("555555555555555555555\n");
//                 case 0x04://返航
//                     send_data[5] = 0x44;
//                     break;  //返航
//                 case 0x05://速度控制
//                     send_data[5] = 0x23;
//                     send_data[6] =(  (int16_t) (para->y_velocity*84)  )   & 0xff;
//                     send_data[7] =(  (int16_t) (para->y_velocity*84)  ) >>8 & 0xff;

//                     send_data[8] =(  (int16_t) (para->x_velocity*84)  ) & 0xff;
//                     send_data[9] =(  (int16_t) (para->x_velocity*84)  ) >>8 & 0xff;
                    
//                     send_data[10] =(  (int16_t)(para->z_velocity*280)  ) & 0xff;
//                     send_data[11] =(  (int16_t) (para->z_velocity*280)  ) >>8 & 0xff;
                    
//                     send_data[12] =(  (int16_t) (para->yaw_rate*16)  ) & 0xff;
//                     send_data[13] =(  (int16_t) (para->yaw_rate*16)  ) >>8 & 0xff;
//                     break;    //切换到P模式，且速度指令均为0
//                 case 0x07://自动执行任务
//                     send_data[5] = 0x43;break;   //任务只有“开始导航”
//                 case 0x08://继续导航
//                     send_data[5] = 0x43;//继续执行任务
//                     break;
//                 case 0x09://开始自检
//                     usr_flag.self_check_flag = false;
//                     break;//自检标志位清零，重新检测
//                 case 0x10:
//                     // usr_flag.onboard_land_flag  = true;
//                     break;


//                 case 0x27: //加锁
//                 send_data[5] = 0x21; 
//                 // printf("auto  lock!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!auto lock\n");

//                     break;
//                 default:
//                     break;
//             }
//         usr_flag.who_control_flight = false;
//     }
//     else
//     {
//        // std::cout << "11111z111111111111111111111" << std::endl;
//         usr_flag.who_control_flight = true;           ////true:remote control
//     }



//     }//usr_flag not lu`ose control


//     if(usr_flag.who_control_flight == false)    //遥控器不动，则听地面站的话
//     {
        
//     }
//     else //遥控器动了，则地面站失效
//     {
//         //most: 1600 lest:400 middle:1000
//         if(!(usr_flag.remote_lose_rc_link_flag))
//         {
//         groundbase_cmd.control_cmd = 1;
//         usr_flag.who_control_flight = true;


//         reset_onboard_target();

    

//         if(remote_cmd.UAV_acc >= 1600)
//             remote_cmd.UAV_acc = 1600;
//         else if(remote_cmd.UAV_acc <= 400)
//             remote_cmd.UAV_acc = 400;

//         if(remote_cmd.UAV_pitch >= 1600)
//             remote_cmd.UAV_pitch = 1600;
//         else if(remote_cmd.UAV_pitch <= 400)
//             remote_cmd.UAV_pitch = 400;

//         if(remote_cmd.UAV_roll >= 1600)
//             remote_cmd.UAV_roll = 1600;
//         else if(remote_cmd.UAV_roll <= 400)
//             remote_cmd.UAV_roll = 400;

//         if(remote_cmd.UAV_yaw >= 1600)
//             remote_cmd.UAV_yaw = 1600;
//         else if(remote_cmd.UAV_yaw <= 400)
//             remote_cmd.UAV_yaw = 400;
        
//         send_data[5] = 0x23;

      
//         if((remote_cmd.UAV_acc>970)&&(remote_cmd.UAV_acc<1030)&&(remote_cmd.UAV_pitch>970)&&(remote_cmd.UAV_pitch<1030)&&(remote_cmd.UAV_roll>970)&&(remote_cmd.UAV_roll<1030)&&(remote_cmd.UAV_yaw>970)&&(remote_cmd.UAV_yaw<1030))
//         {
//         send_data[6] = 0;
//         send_data[7] = 0;
//         send_data[8] = 0;
//         send_data[9] = 0;

//         send_data[10] = 0;
//         send_data[11] = 0;
//         send_data[12] = 0;
//         send_data[13] = 0;

//         }else{
            
//         send_data[6] =(  (int16_t) (((remote_cmd.UAV_roll -1000)/600.0)*840) ) & 0xff;
//         send_data[7] =(  (int16_t) (((remote_cmd.UAV_roll -1000)/600.0)*840) ) >>8 & 0xff;

//         send_data[8] =(  (int16_t) (((remote_cmd.UAV_pitch -1000)/600.0)*840) ) & 0xff;
//         send_data[9] =(  (int16_t) (((remote_cmd.UAV_pitch -1000)/600.0)*840) ) >>8 & 0xff;
        
//         send_data[10] =(  (int16_t) (((remote_cmd.UAV_acc -1000)/600.0)*840) ) & 0xff;
//         send_data[11] =(  (int16_t) (((remote_cmd.UAV_acc -1000)/600.0)*840) ) >>8 & 0xff;
        
//         send_data[12] =(  (int16_t) (((remote_cmd.UAV_yaw -1000)/600.0)*840) ) & 0xff;
//         send_data[13] =(  (int16_t) (((remote_cmd.UAV_yaw -1000)/600.0)*840) ) >>8 & 0xff;
        
       

//             printf("send data = %d\n",(  (int16_t) (((remote_cmd.UAV_roll -1000)/600.0)*840) ));

//         }



//             if(remote_cmd.UAV_land>1050)
//             {       
//                 send_data[5] = 0x41; 
//                 send_data[6] = 0;
//                 send_data[7] = 0;
//                 send_data[8] = 0;
//                 send_data[9] = 0;

//                 send_data[10] = 0;
//                 send_data[11] = 0;
//                 send_data[12] = 0;
//                 send_data[13] = 0;
                                 
//             }

//         }

//     }
//     // printf("send_data[5] = %d\n",send_data[5]);
//     send_data[14] = (para -> pan_tilt_pitch)&0xFF;
//     send_data[15] = ((para -> pan_tilt_pitch) >>8)&0xFF;
//     send_data[16] = (para -> pan_tilt_point_to)&0xFF;
//     send_data[17] = ((para -> pan_tilt_point_to) >>8)&0xFF;
    
//     crc = CalculateCrc16(send_data,18);
    
//     send_data[18] = (crc >> 8)&0xFF;
//     send_data[19] = crc&0xFF;
     
//             radio->send(send_data, index);

//              if( usr_flag.remote_lose_rc_link_flag)     ///give gcs the controler
//             {
//                   usr_flag.remote_control_flag = false;   
//                     usr_flag.who_control_flight = false;    
//             }

//         last_groundbase_cmd = para -> control_cmd;

//     return index;
    
// }
/********************************************************************************************************************************************8
 * 111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111*/
uint8_t NX_serial_send_to_flight_control(LinuxSerialDevice* radio, groundbase_cmd_t* para)      //NX向飞控发送控制包
{
    static uint16_t crc;
    static uint8_t index = 20;
    static uint8_t send_data[20] = {0};
    int16_t GPS_control =0;
    uint8_t last_groundbase_cmd = 0;
    static uint8_t i = 0;

    static uint8_t sum_send_time = 0;

    send_data[0] = 0x43;
    send_data[1] = 0x57;
    send_data[2] = 13;
    send_data[3] = 0x00;
    send_data[4] = 0x01;

    if( (usr_flag.remote_lose_rc_link_flag) && (usr_flag.remote_control_flag) )
    {
         send_data[5] = 0x44;
         //roll
          send_data[6] = 0;
          send_data[7] = 0;
        //pitch
          send_data[8] = 0;
          send_data[9] = 0;
        //throttle
          send_data[10] = 0;
          send_data[11] = 0;
          //yaw
          send_data[12] = 0;
          send_data[13] = 0;

    }
    else
    {
    if((!usr_flag.remote_control_flag))       ///gcs control     //遥控器没动  
        {

            if(flight_control_lose_massage_flag)
            {
                para -> control_cmd = 0x04;   
                //   printf("+++++++++++++++++++++++jjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjjj++++++++\n");
            }

            // if(onboard_land_state == Landed){
            //     para -> control_cmd = 0x27;   
            // }
            if((usr_flag.NX_get_groundbase_heartbeat_cnt >= 3) && (usr_flag.gcs_lose_heart_beat_flag))   ///lose gcs heartbeat
            {
                para -> control_cmd = 0x04;   
                // reset_onboard_target();

                usr_flag.NX_get_groundbase_heartbeat_cnt = 0;
            }

                switch (para ->control_cmd)
                {
                    case 0x00://解锁
                        send_data[5] = 0x20;
                        break;// 起浆
                    case 0x02://起飞
                        // send_data[5] = 0x40;
                        usr_flag.onboard_land_flag  = true;
                        break;   //自动起飞
                    case 0x03://降落
                        send_data[5] = 0x41;
                        usr_flag.onboard_land_flag  = false;
                        break;  //自动降落
                    case 0x04://返航
                        send_data[5] = 0x44;
                        usr_flag.onboard_land_flag  = false;
                        break;  //返航
                    case 0x05://
                        send_data[5] = 0x23;
                        send_data[6] = 0;
                        send_data[7] = 0;
                        //pitch
                        send_data[8] = 0;
                        send_data[9] = 0;
                        //throttle
                        send_data[10] = 0;
                        send_data[11] = 0;
                        //yaw
                        send_data[12] = 0;
                        send_data[13] = 0;
                        usr_flag.onboard_land_flag  = false;
                        break;    
                    case 0x07://自动执行任务
                        send_data[5] = 0x43;
                        break;   //任务只有“开始导航”
                    case 0x08://继续导航
                        send_data[5] = 0x43;//继续执行任务
                        break;
                    case 0x09://开始自检
                        usr_flag.self_check_flag = false;
                        break;//自检标志位清零，重新检测

                    //未完成，地面站控制命令
                    case 0x10:
                        // usr_flag.onboard_land_flag  = true;//
                        break;
                    case 0x27: //加锁
                    send_data[5] = 0x21; 
//                 // printf("auto  lock!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!auto lock\n");
                        break;
                    default:
                        break;
                }
            usr_flag.who_control_flight = false;
        }
        else
        {
            std::cout << "11111z111111111111111111111yaokongqi" << std::endl;
            usr_flag.who_control_flight = true;           ////true:remote control
        }
    }//usr_flag not lu`ose control

    if(usr_flag.who_control_flight == false)    //遥控器不动，则听控制程序的话
    {
        if(usr_flag.onboard_land_flag)//进入板载控制模式
        {
            switch (autocontrol_flag)
            {
                case 0x01://解锁
                    send_data[5] = 0x20;
                    break;// 起浆
                case 0x02://起飞
                    send_data[5] = 0x40;  
		    break;                
                case 0x03://板载控制
                    send_data[5] = 0x23;
                    send_data[6] =desired_vel_y & 0xff;
                    send_data[7] =desired_vel_y >>8 & 0xff;

                    send_data[8] =desired_vel_x & 0xff;
                    send_data[9] =desired_vel_x >>8 & 0xff;
                    
                    send_data[10] =desired_vel_z & 0xff;
                    send_data[11] =desired_vel_z >>8 & 0xff;
                    
                    send_data[12] =desired_yaw_rate & 0xff;
                    send_data[13] =desired_yaw_rate >>8 & 0xff;
                    break;
                case 0x04://起飞
                    send_data[5] = 0x41;
                    break;                    
                default:
                    break;
            }            
        }


    }
    else //遥控器动了，则控制程序失效
    {
        //most: 1600 lest:400 middle:1000
        if(!(usr_flag.remote_lose_rc_link_flag))
        {
            // groundbase_cmd.control_cmd = 1;
            usr_flag.who_control_flight = true;


            // reset_onboard_target();

            if(remote_cmd.UAV_acc >= 1600)//限幅
                remote_cmd.UAV_acc = 1600;
            else if(remote_cmd.UAV_acc <= 400)
                remote_cmd.UAV_acc = 400;

            if(remote_cmd.UAV_pitch >= 1600)
                remote_cmd.UAV_pitch = 1600;
            else if(remote_cmd.UAV_pitch <= 400)
                remote_cmd.UAV_pitch = 400;

            if(remote_cmd.UAV_roll >= 1600)
                remote_cmd.UAV_roll = 1600;
            else if(remote_cmd.UAV_roll <= 400)
                remote_cmd.UAV_roll = 400;

            if(remote_cmd.UAV_yaw >= 1600)
                remote_cmd.UAV_yaw = 1600;
            else if(remote_cmd.UAV_yaw <= 400)
                remote_cmd.UAV_yaw = 400;
            
            send_data[5] = 0x23;

        
            if((remote_cmd.UAV_acc>970)&&
                    (remote_cmd.UAV_acc<1030)&&
                        (remote_cmd.UAV_pitch>970)&&
                            (remote_cmd.UAV_pitch<1030)&&
                                (remote_cmd.UAV_roll>970)&&
                                    (remote_cmd.UAV_roll<1030)&&
                                        (remote_cmd.UAV_yaw>970)&&
                                            (remote_cmd.UAV_yaw<1030))
            {
                send_data[6] = 0;
                send_data[7] = 0;
                send_data[8] = 0;
                send_data[9] = 0;

                send_data[10] = 0;
                send_data[11] = 0;
                send_data[12] = 0;
                send_data[13] = 0;
            }
            else
            {  
                send_data[6] =(  (int16_t) (((remote_cmd.UAV_roll -1000)/600.0)*840) ) & 0xff;
                send_data[7] =(  (int16_t) (((remote_cmd.UAV_roll -1000)/600.0)*840) ) >>8 & 0xff;

                send_data[8] =(  (int16_t) (((remote_cmd.UAV_pitch -1000)/600.0)*840) ) & 0xff;
                send_data[9] =(  (int16_t) (((remote_cmd.UAV_pitch -1000)/600.0)*840) ) >>8 & 0xff;
                
                send_data[10] =(  (int16_t) (((remote_cmd.UAV_acc -1000)/600.0)*840) ) & 0xff;
                send_data[11] =(  (int16_t) (((remote_cmd.UAV_acc -1000)/600.0)*840) ) >>8 & 0xff;
                
                send_data[12] =(  (int16_t) (((remote_cmd.UAV_yaw -1000)/600.0)*840) ) & 0xff;
                send_data[13] =(  (int16_t) (((remote_cmd.UAV_yaw -1000)/600.0)*840) ) >>8 & 0xff;
                printf("send data = %d\n",(  (int16_t) (((remote_cmd.UAV_roll -1000)/600.0)*840) ));

            }

            if(remote_cmd.UAV_land>1050)
            {       
                send_data[5] = 0x41; 
                send_data[6] = 0;
                send_data[7] = 0;
                send_data[8] = 0;
                send_data[9] = 0;

                send_data[10] = 0;
                send_data[11] = 0;
                send_data[12] = 0;
                send_data[13] = 0;
                                 
            }

        }
    //遥控器动了，则地面站失效
    }
    // printf("send_data[5] = %d\n",send_data[5]);
    send_data[14] = 0x00;
    send_data[15] = 0x00;
    send_data[16] = 0x00;
    send_data[17] = 0x00;
    crc = CalculateCrc16(send_data,18);
    
    send_data[18] = (crc >> 8)&0xFF;
    send_data[19] = crc&0xFF;
            radio->send(send_data, index);

    if( usr_flag.remote_lose_rc_link_flag)     ///give gcs the controler
    {
            usr_flag.remote_control_flag = false;   
            usr_flag.who_control_flight = false;    
    }

    return index;
    
}

uint8_t NX_serial_send_to_groundbase_of_waypoint_ACK(LinuxSerialDevice* radio)   //给地面站的ACK
{
    uint8_t i = 0,send_number = 0;
    uint8_t send_data[25] = {0};
    uint8_t index = 0;
    uint16_t check_sum = 0;

        
     if(NX_get_start_up_waypoint_flag && NX_get_up_waypoint_mess_flag)              ///请求上传航点接收成功，且已经得到飞控的航点总数
    {
        NX_get_up_waypoint_mess_flag = false;
        NX_get_start_up_waypoint_flag = false ;

        send_data[0] = 0xEB;
        send_data[1] = 0x90;
        send_data[2] = 8;
        send_data[3] = 0xB4;
        send_data[4] = usr_flag.flight_ID;
        send_data[5] = flight_control_waypoint.point_number;

        for(i=0;i<6;i++)
        {
            check_sum += send_data[i];
        }
        send_data[6] = (check_sum>>8)&0xFF;
        send_data[7] = check_sum&0xFF;

         index = 8;
           printf("up hangdian totle number = %d\n",flight_control_waypoint.point_number);
       // for(send_number = 0;send_number < 5;send_number++)  //发五帧
      //  {
            // radio->send(send_data, index);
            UDP_send(send_data,UDP_fd,index);
      //  }
        

        check_sum = 0;
    } else if(NX_get_up_waypoint_mess_flag) 
    {
         NX_get_up_waypoint_mess_flag = false;

        send_data[0] = 0xEB;
        send_data[1] = 0x90;
        send_data[2] = 0x17;
        send_data[3] = 0xB5;
        send_data[4] = usr_flag.flight_ID;
            printf("up single2222 = %d\n",flight_control_waypoint.current_index);
        send_data[5] = flight_control_waypoint.current_index+1;  ///航点序号
        send_data[6] = flight_control_waypoint.point_mode;     ///航点类型

         printf("up single3333 = %d\n",flight_control_waypoint.current_index);

        int32_t waypoint_lat = (int32_t)(flight_control_waypoint.point_i_lat*10000000 +(int32_t)(flight_control_waypoint.point_f_lat/100));
        printf("waypoint_lat= %d\n",waypoint_lat);
        send_data[7] = (waypoint_lat>>24)&0xFF ;
        send_data[8] = (waypoint_lat>>16)&0xFF;
        send_data[9] = (waypoint_lat>>8)&0xFF;
        send_data[10] = waypoint_lat&0xFF;

        int32_t waypoint_lng = (int32_t)(flight_control_waypoint.point_i_lon*10000000 + (int32_t)(flight_control_waypoint.point_f_lon/100));
         printf("waypoint_lng= %d\n",waypoint_lng);
        send_data[11] =  (waypoint_lng>>24)&0xFF;
        send_data[12] = (waypoint_lng>>16)&0xFF;
        send_data[13] = (waypoint_lng>>8)&0xFF;
        send_data[14] =waypoint_lng&0xFF;

        int32_t waypoint_alt = (int32_t)((flight_control_waypoint.point_altitude)/10);    //飞控数据单位为mm    地面站需要cm
        send_data[15] = (waypoint_alt>>24)&0xFF;
        send_data[16] = (waypoint_alt>>16)&0xFF;
        send_data[17] =(waypoint_alt>>8)&0xFF;
        send_data[18] = waypoint_alt&0xFF;

        int32_t waypoint_time = (int32_t)(flight_control_waypoint.point_duration);    //悬停时间
        send_data[19] =(waypoint_time>>8)&0xFF ;
        send_data[20] = waypoint_time&0xFF;
        

        for(i=0;i<21;i++)
        {
            check_sum += send_data[i];
        }
        send_data[21] = (check_sum>>8)&0xFF;
        send_data[22] = check_sum&0xFF;

         index = 23;
        //for(send_number = 0;send_number < 5;send_number++)  //发五帧////待调整，应该要改成一直发，直到收到地面站请求下一个航点信息
      //  {
            // radio->send(send_data, index);
            UDP_send(send_data,UDP_fd,index);

            if(flight_control_waypoint.current_index == flight_control_waypoint.point_number-1)
            {
                NX_get_up_waypoint_final_flag = true;
                 if_up_and_down_waypoint_falg = false;
            }
           printf("up single444444444444 = %d\n",flight_control_waypoint.current_index);
              printf("up hhhhhh = %d\n",flight_control_waypoint.point_number);
      //  }
         
        

        check_sum = 0;

        if(NX_get_up_waypoint_final_flag)
        {
        NX_get_up_waypoint_final_flag= false;
        send_data[0] = 0xEB;
        send_data[1] = 0x90;
        send_data[2] = 0x08;
        send_data[3] = 0xB4;
        send_data[4] = usr_flag.flight_ID;
        send_data[5] = 0xF0;  ///航点全部上传完毕

        for(i=0;i<6;i++)
        {
            check_sum += send_data[i];
        }
        send_data[6] = (check_sum>>8)&0xFF;
        send_data[7] = check_sum&0xFF;

   printf("up single45555555555 = %d\n",if_up_and_down_waypoint_falg);
    //   if_up_and_down_waypoint_falg = false;

         index = 8;
        for(send_number = 0;send_number < 5;send_number++)  //发五帧////待调整，应该要改成一直发，直到收到地面站请求下一个航点信息
        {
            // radio->send(send_data, index);
            UDP_send(send_data,UDP_fd,index);
        }
        

        check_sum = 0;

        }

    } 


    if(waypoints_tracking_flag == true)//跟随航点命令接受成功
    {
        waypoints_tracking_flag = false;
        send_data[0] = 0xEB;
        send_data[1] = 0x90;
        send_data[2] = 9;
        send_data[3] = 0xB3;
        send_data[4] = usr_flag.flight_ID;
        send_data[5] = 0xff;
        send_data[6] = 0;
        for(i=0;i<7;i++)
        {
            check_sum += send_data[i];
        }
        send_data[7] = (check_sum>>8)&0xFF;
        send_data[8] = check_sum&0xFF;

        index = 9;
        for(send_number = 0;send_number < 5;send_number++)  //发五帧
        {
            // radio->send(send_data, index);
            UDP_send(send_data,UDP_fd,index);
            //std::cout << "000000000000000000000000000" << std::endl;
        }
        
        for(i=0;i<9;i++)
        {
            // printf("send_data[%x] = %d\n",i,send_data[i]);
            UDP_send(send_data,UDP_fd,index);
        }
        //printf("check_sum = %d\n",check_sum&&0xFF);
        check_sum = 0;
        
    }

    if(waypoints_cmd_flag == true)//航点命令接受成功
    {
        waypoints_cmd_flag = false;
        send_data[0] = 0xEB;
        send_data[1] = 0x90;
        send_data[2] = 9;
        send_data[3] = 0xB3;
        send_data[4] = usr_flag.flight_ID;
        send_data[5] = 0x01;
        send_data[6] = 0;
        for(i=0;i<7;i++)
        {
            check_sum += send_data[i];
        }
        send_data[7] = (check_sum>>8)&0xFF;
        send_data[8] = check_sum&0xFF;

        index = 9;
        NX_get_up_down_waypoint_cnt = 0;
        for(i=0;i<9;i++)
        {
            printf("send_datasssssssssssss[%x] = %d\n",i,send_data[i]);
        }
       // for(send_number = 0;send_number < 5;send_number++)  //发五帧
       // {
            // radio->send(send_data, index);
            UDP_send(send_data,UDP_fd,index);
            std::cout << "000000000000000000000000000" << std::endl;
      //  }
        
      
        //printf("check_sum = %d\n",check_sum&&0xFF);
        check_sum = 0;
      
        
    }



    if(NX_get_up_down_waypoint_cnt >=3 && if_up_and_down_waypoint_falg)//航点命令接受chaoshi
    {
        NX_get_up_down_waypoint_cnt = 0;
        if_up_and_down_waypoint_falg = false;

    }

    if((waypoints_single_flag == true)&&(usr_flag.flight_point_flag == true))//((usr_flag.flight_point_flag == true)&&(usr_flag.final_waypoint_flag == false))   //单个航点接收成功
    {
       
        waypoints_single_flag =false;
        usr_flag.flight_point_flag = false;
        send_data[0] = 0xEB;
        send_data[1] = 0x90;
        send_data[2] = 9;
        send_data[3] = 0xB3;
        send_data[4] = usr_flag.flight_ID;
        send_data[5] = 0x02;
        send_data[6] = groundbase_waypoint.current_index + 1;
        for(i=0;i<7;i++)
        {
            check_sum += send_data[i];
        }
        send_data[7] = (check_sum>>8)&0x00FF;
        send_data[8] = check_sum&0x00FF;
        
        index=9;

        for(i=0;i<9;i++)
        {
            printf("send_data_to_groundbase[%x] = %d\n",i,send_data[i]);
        }
       // for(send_number = 0;send_number < 10;send_number++)  //发五帧
       // {
             NX_get_up_down_waypoint_cnt = 0;
            // radio->send(send_data, index);
            UDP_send(send_data,UDP_fd,index);
            printf("singlewaypoint111111111111111111111111111111111 = %d\n",send_data[6]);
             //std::cout << "singlewaypoint111111111111111111111111111111111" << std::endl;
    //   }
        
    
        check_sum = 0;
              
    }

    if(all_waypoint_OK == true)            //所有航点接收成功
    {
        all_waypoint_OK = false;
        send_data[0] = 0xEB;
        send_data[1] = 0x90;
        send_data[2] = 9;
        send_data[3] = 0xB3;
        send_data[4] = usr_flag.flight_ID;
        send_data[5] = 0x03;
        send_data[6] = groundbase_waypoint.current_index;
        for(i=0;i<7;i++)
        {
            check_sum += send_data[i];
        }
        send_data[7] = (check_sum>>8)&0x00FF;
        send_data[8] = check_sum&0x00FF;
        
          NX_get_up_down_waypoint_cnt = 0;
        index=9;
        for(send_number = 0;send_number < 5;send_number++)  //发五帧
        {
            // radio->send(send_data, index);
            UDP_send(send_data,UDP_fd,index);
            std::cout << "aaaaaaaalllllllllllllllllllOOOOOOOOKKKKKKKKK" << std::endl;
        }
        if_up_and_down_waypoint_falg = false;
        //groundbase_waypoint.next_index+=1;
        
        check_sum = 0;
    }

    check_sum = 0;
    //   printf("33333333333333333333333\n");
    return index;

}

uint8_t NX_serial_send_to_flight_control_query(LinuxSerialDevice* radio, groundbase_cmd_t* para)      ///航点查询包
{
    static uint16_t crc;
    static uint8_t index = 10;
    static uint8_t send_data[10] = {0};
    //static uint8_t i = 0;

    send_data[0] = 0x43;
    send_data[1] = 0x57;
    send_data[2] = 3;
    send_data[3] = 0x00;
    send_data[4] = 0x00;
    send_data[5] = para -> query_cmd;
    if(para -> query_cmd == 0x08)
    {
        printf("kkkgroundbase_cmd.current_up_index= %d\n",groundbase_cmd.current_up_index);
       //groundbase_cmd.current_up_index = 1;
         send_data[6] = groundbase_cmd.current_up_index&0xFF;
        send_data[7] = (groundbase_cmd.current_up_index>>8)&0xFF;

    }
    else 
    {
        send_data[6] = 0x00;
        send_data[7] = 0x00;
    }

    crc = CalculateCrc16(send_data,8);
    
    send_data[8] = (crc >> 8)&0xFF;
    send_data[9] = crc&0xFF;

    radio->send(send_data, index);
    return index;
}



uint8_t user_send_waypoints_cmd_to_flightcontrol(LinuxSerialDevice* flightcontrol)      //航点通信-航点控制指令  （飞控收）
{
    uint8_t index = 0;
    const uint8_t length = 0x09;
    uint8_t buffer[length] = {0};
    index = set_head(buffer,index,0xeb,0x90,length,0xB1,usr_flag.flight_ID);
    buffer[index++] = waypoints_cmd[0];
    buffer[index++] = waypoints_cmd[1];
    buffer[index++] = waypoints_cmd[2];
    buffer[index++] = waypoints_cmd[3];

    flightcontrol->send(buffer, index);

    return index;
}
uint8_t user_send_waypoints_single_to_flightcontrol(LinuxSerialDevice* flightcontrol)    //航点通信-单条航点信息（飞控收）
{
    uint8_t index = 0;
    const uint8_t length = 0x17;
    uint8_t buffer[length] = {0};
    index = set_head(buffer,index,0xeb,0x90,length,0xB2,usr_flag.flight_ID);
    for(uint8_t i=0;i<18;i++)
    {
        buffer[5+i] = waypoints_single[i];
        index++;
    }
    flightcontrol->send(buffer, index);

    return index;
}
